The present invention relates to sensing and tracking the position of objects in an environment, particularly but not exclusively an environment where the features of the environment are not well controlled or predefined.
In order to undertake tasks in an environment that is known or unknown a priori, systems controlling machinery which must move in or through the environment, for example mining equipment, must have knowledge of the absolute or true position and orientation of the machinery in relation to the surroundings of the machinery. For some instances of controlled operation whereby elements of the control are exercised by a human operator and for autonomous operation, a control system must be equipped with a means to determine the absolute or true position and orientation of the machinery and the complete relationship of the machinery to the surroundings it is operating in.
The environment may be composed of natural or artificial features and may be complex thus possessing little or no regular structure. In addition, some features in the surroundings may be in motion relative to other features nearby.
Many methods have been disclosed, which enable a machine to navigate (determine its position and control its position) by using artificial features in a predetermined environment. For example, U.S. Pat. No. 4,831,539 to Hagenbuch discloses a system for identifying the location of a vehicle using distinctive signposts located at predefined positions. U.S. Pat. No. 4,811,228 to Hyyppa and U.S. Pat. No. 5,367,458 to Roberts utilise predefined target or signpost devices which are identified in some way by the vehicle to provide location information.
U.S. Pat. No. 5,051,906 to Evans, Jr., et al discloses an apparatus and method which provides for the determination of a vehicle""s orientation and position in an environment, such as a hallway, from an image of a retroreflective ceiling feature.
These systems are inherently limited in their application by the requirement to determine the environment prior to operation of the machine and in many cases install artificial features which are used to determine the position of the machine within the defined environment. For example, it may be possible in a warehouse environment to provide a well controlled environment without unexpected featuresxe2x80x94however, this is much more difficult in a changing environment such as an open cut mine or underground mine, where the shape and location of objects are inherently in a state of change.
Other methods which enable a machine to navigate using inertial navigation systems have been disclosed. The operation of inertial navigation systems is usually based on assumptions about the reference framexe2x80x94for example the rotation and revolution of the earth. Double integration of the acceleration determined by the navigation system often results in unacceptable drift in the calculated position determined by the inertial navigation system. Also, repeated changes in acceleration and repeated movement about a point tend to produce cumulative errors in inertial systems, as the next position assessment is based upon the previously determined value.
U.S. Pat. No. 4,626,995 to Lofgren et al discloses an arrangement in which a computer determines the Cartesian coordinates of a single light source through a camera attached to a vehicle. This arrangement requires that the height of the light source and the height of the sensor be predetermined. U.S. Pat. No. 4,858,132 to Holmquist discloses a system in which a computer determines the bearing of a composite light source through a camera attached to a vehicle, and from the apparent spacing between the elements of the lights determines bearing and range.
U.S. Pat. No. 5,483,455 discloses an arrangement in which targets located at predefined positions with respect to a base reference frame are detected and the position of a vehicle relative to the target determined. Location of the vehicle relative to the base reference frame is determined from the position of the vehicle relative to the known target.
It has been disclosed in the prior art that laser scanners may be used to determine position on a known path relative to a defined set of fixed reference points. In one embodiment, a laser or light transmitter scans a volume in which are located characteristic features consisting of reflectors intended to direct the emitted light back to a sensor located with the transmitter. The prior art also teaches the use of laser scanners to determine position relative to natural features and to memorise the position of such features in a two dimensional plane only in order to navigate between them.
It is also disclosed in the prior art to use scanning laser rangefinders to position equipment 2-dimensionally in a constrained environment such as a tunnel. Such techniques, known as wall following, are used for obstacle detection and the avoidance of collisions with fixed features such as walls and other obstacles. Such techniques may also be used to fuse data referenced to a known local environment with data from a dead reckoning system such as an inertial navigation system (INS) by periodically resetting the INS see xe2x80x9cExperiments In Autonomous Underground Guidancexe2x80x9d, Scheding S., Nebot E., Stevens M., Durrant-Whyte H., Roberts J., Corke P., Cunningham J., Cook B; in IEEE Conference on Robotics and Automation, Albuquerque 1997.
In xe2x80x9cAn Experiment in Guidance and Navigation of an Autonomous Robot Vehiclexe2x80x9d, Bianche IEEE Transactions on Robotics and Automation, Vol 7 , No 2, April 1991, an experimental vehicle designed to operate autonomously within a structured office or factory environment is discussed. The disclosed device uses an odometry and steering angle based primary system, with a laser rangefinder used to correct this with respect to a predetermined 2-D map of the environment, and remotely generated path plans.
Methods have been disclosed also, which enable a machine to avoid collision with features in its environment, obstacle avoidance, that is, to determine its position and control its position relative to those features in such a manner as to avoid contact with the features. For example, U.S. Pat. No. 5,758,298 to Guldner discloses an autonomous navigation system for a mobile robot or manipulator. In the description of this patent all operations are performed on the local navigation level in the robot coordinate system. U.S. Pat. No. 4,954,962 to Evans, Jr., et al discloses a navigation control system of a robot which inputs data from a vision system and infers therefrom data relating to the configuration of the environment which lies in front of the robot so that the robot may navigate to a desired point without collision with obstacles or features in its environment.
Obstacle avoidance or collision avoidance systems are not required to determine and track the true position and orientation of the machine within the defined environment and therefore cannot be used for navigation and guidance except in the very local sense of avoiding collisions.
A plurality of methods have been disclosed involving the use of methods such as predetermined or installed reference points, stored maps of the local environment, infrastructure such as the Global Positioning System (GPS) or local radio navigation systems and systems such as inertial navigation systems. All of these methods use infrastructure which may be integral with the immediate environment or external to the immediate environment and must exist or be installed.
It is an object of the present invention to provide a location and navigation system which will enable a machine to operate over an extended area knowing its true location in that area which does not require the use of a predetermined reference frame or network of reference features, and is not reliant upon INS or GPS or similar sensing arrangements.
According to a first aspect, the present invention provides a method for determining the position of a movable object, including the steps of:
(a) initiating the process of determining the absolute or true position of said object in 3 dimensions when said object is at a known position in 3 dimensions
(b) obtaining data indicative of the 3 dimensional location of one or more fixed features which may then be used as reference locations relative to said object via a sensing means, that is determining from said data the position in three dimensions of said one or more fixed features with respect to said object;
(c) moving said object;
(d) at the new position, obtaining data indicative of the new location in 3 dimensions of said one or more fixed features relative to said object via said sensing means, and determining from said data the displacement of said object with respect to said one or more fixed features;
(e) determining from said displacement of the object and said knowledge of the initial (or previous) position of the object the new position of the object in three dimensions;
(f) obtaining additional data indicative of the 3 dimensional location relative to said object of said fixed features or obtaining data indicative of extensions of said fixed features not previously visible to the sensors and
(g) when possible or necessary i.e. if the object is moving out of view of previously known fixed reference features or when new fixed references are in view, obtaining data indicative of the 3 dimensional location of one or more new fixed features which may be used as new reference locations (which are different to the aforementioned ones) relative to said object via said sensing means, that is determining from said data the position of said one or more new fixed reference locations with respect to said object; and
(h) repeating steps (c) to (g) as required until predefined conditions regarding the location of the object are fulfilled.
Conveniently, a 3-D imaging sensor, which provides measurements of the true relative position in 3-D space of objects relative to the sensor and may additionally provide measurements of the optical reflectance of objects in the field of view (such as is acquired in a camera image or seen with the eye) such optical reflectance being registered with the spatial information collected by the sensor, is mounted on the movable object with a predetermined (known) orientation and position thereon. The reference locations may form part of a natural feature such as a definite surface and/or define such feature, as well as artificial bodies with features defining a 3-D object.
According to another aspect, the present invention provides an autonomous vehicle, said vehicle including:
drive means for selectively moving said vehicle;
sensor means mounted at a known orientation and position on said vehicle, said sensor means providing 3-D imaging data representative of at least a selected volume about said vehicle;
processing means for receiving said sensor data, processing said sensor data in accordance with a pre-determined instruction set so as to locate one or more fixed points, determining the position of said vehicle with respect to said fixed points in the selected volume, and so determine the position and orientation of said vehicle and generate control signals for said drive means.
The invention further includes apparatus enabled to implement the invention, and a system for controlling a set of autonomous vehicles using the inventive method.
The invention relates to a method for using knowledge of the position of the sensor, in three dimensions, relative to a known reference or fixed features to track the position of the sensor in three dimensions over a period of time. The sensor produces data in three dimensions which are measurements of the distance and bearing to objects in the field of regard of the sensor; that is the sensor is a xe2x80x98three dimensional imaging systemxe2x80x99. The data is used to determine the position of the sensor relative to fixed features in the surroundings of the sensor. Knowledge of the position of the sensor relative to a fixed object or objects in three dimensions completely determines the local position of the sensor. To measure movement, the position of the sensor relative to a fixed object or set of objects must be known in three dimensions at one time and must then be determined at a second time. The movement of the object is then determined directly from the difference between the two positions. Motion of the sensor may be tracked by successive determinations of the change in position of the sensor. The sensor may be mounted on equipment such as a vehicle or other machinery and used to track the movement of the equipment.
The movement of the sensor can be determined from changes in position relative to natural features or characterising features intended to provide specific reference points for guidance. The three dimensional spatial relationship of the sensor and features in the sensor environment is the key data used to track the sensorxe2x80x94not a predetermined map or an external reference system. Changes in position may be determined relative to a fixed point or a set of fixed points. The total movement of the sensor over a period of time can be determined by determining the position of the sensor at a succession of times. When starting from a known or predetermined position, the true position of the sensor at a specific time can be determined from knowledge of the starting position and knowledge of the movement of the sensor from the starting position up to that time.
A fixed object or a succession of fixed objects is used to determine the position of the moving sensor at successive times and the absolute position of the moving sensor relative to the known position from which it started may be determined at each of these times and the moving sensor thereby tracked. An object, for instance a vehicle, fitted with a three dimensional sensor system and a processing system to execute the algorithm can therefore track the position in three dimensions of the object carrying the sensor using knowledge of the starting position and knowledge of the movement in three dimensions. When fitted with a suitable three dimensional sensor and processing system the object can track its motion and determine its position relative to its starting point without requiring transmission of information to the object such as transmissions of radio waves as used in the Global Positioning System (GPS) or sensing elements of the motion of the object such as velocity or acceleration as used in inertial guidance systems. The three dimensional information acquired by the sensor consists of spatial measurements and embodies no scaling of the data. The representation of features therefore provides information as to the position and structure of objects in the field of view directly.
The invention described is a means of using knowledge of the three dimensional position relative to the surroundings to track the motion and thus navigate from a known starting position or relative to sensed features known to be fixed. The knowledge of the relative three dimensional position is obtained using a three dimensional imaging system.
The object may be, for example, an autonomous vehicle, wherein the location method described is used for navigation. The reference location may be predefined elements in whole or part where the environment is well defined. However, they may equally be determined by an instruction set (software) provided on the vehicle, according to some criteria to ensure the feature will be locatable after subsequent movement.
The sensing arrangement may be any suitable sensor which provides a direct indication of the absolute position or displacement of points relative to the sensor. As the arrangement of the sensor on the object is known, the sensing arrangement can also provide data about the orientation or attitude of the object, as the orientation of the reference feature will change with changes in the object orientation.
It will be appreciated that a key advantage of the inventive arrangement is that it does not require the environment to be fully defined or fitted with carefully plotted reference points. The inventive method simply selects suitable fixed features or parts of fixed features as reference points in transit. It does not rely on referencing to the predetermined location of the points, and so elaborate set up arrangements as are necessary in the prior art are not required.
It will be appreciated that the present invention includes a method of navigation, wherein location is sensed as above, and appropriate direction, acceleration and velocity decisions are made in accordance with software instructions. Many such proposals are described in the prior artxe2x80x94it is the underlying location scheme which is of key significance and difference to the prior art. In a navigation system, the vehicle will be provided with an intended destination or waypoint defined in three dimensions, conveniently relative to the start position or some agreed reference point.
The inventive arrangement may be used in conjunction with an INS or other positioning system to refine the position estimates and increase the accuracy or reliability of either means of estimating position. The inventive arrangement may be combined with or work with a collision avoidance or obstacle avoidance system to provide a plurality of functions.
Considering an environment such as a mine, the advantages of the present invention will become apparent. In a mine, the floor over which a vehicle travels is not uniform in smoothness, grade or surface composition. Any techniques that rely on assumptions about the operating surface being a plane will not be operative. Sensing based upon steer angle or wheel rotations will be inaccurate, and pot holes and wheel spins will alter the apparent distance travelled. Also, in a mine environment, the location in depth may be as important as the two dimensional location, and hence techniques reliant on range estimation based upon height of a target over a plane will be ineffective.
In an environment such as a mine, the shape of the environment alters on a regular basis, due to the nature of extraction of material from the environment. It is also an environment where extraneous features may be added, due to spillage, new working, etc. Hence, as the inventive arrangement does not require elaborate advance mapping, it is ideally suited to such an environment.
Whilst the prior art teaches to use a sequence of monocular (camera) images for the determination of motion, it does not teach the use of matching of features (which may be natural or artificial) in multiple three dimensional images in all three dimensions to determine the position and orientation of an object in the three dimensional space of the field of view of the sensor, which thus allows determining changes in the position of the sensor in three dimensions and tracking the actual position of the sensor.
Known means of determining position include devices such as ultrasound sensors or other range measurement devices or two dimensional imaging systems such as video cameras. These devices provide information in one or two dimensions directly. The acquisition of information in three dimensions requires data from more than one sensor and, in some cases, extensive computation of the data from a number of sensors. A three dimensional imaging system may be comprised of a combination of such sensors and processors or may be a special purpose three dimensional sensor such as a three dimensional laser range measurement scanning system. For example, two dimensional imaging systems only provide information on the angular relationships between features directly and provide no scale but may provide three dimensional information indirectly. To obtain three dimensional information using a two dimensional imaging system generally-requires a great deal of computation. Position determination is based on the measurement of position relative to a known reference such as terrestrial landmarks or stars or on the determination of movement from a known position as in an inertial navigation system. Position consists of three components. These may be x,y,z coordinates in a Cartesian reference frame or on the surface of the earth these may be latitude, longitude and elevation relative to the geoid. In many applications only one component of the position relative to the known reference can be determined. This component may be the bearing to the reference position as used in navigation from maps using triangulation or the distance to a reference as used in the Global Positioning System where the reference is a satellite in a known orbit. When only one component of relative position to a specified reference is known, complete determination of the position of an object requires the knowledge of this component for a number of reference positions. For example, when navigating by map and compass, two bearings are used to determine the position of the object. Three bearings are used to improve the accuracy of determination of the position of the object. When navigating by map and compass the combination of compass bearing and map provide the estimate of position in three dimensions. When all three components of position relative to a reference position are known, the position of an object is fully determined within some limits imposed by measurement error. A three dimensional imaging system provides knowledge of all three position components of objects in the field of view of the sensor and therefore fully determines the position relative to the sensor of an object in the field of view of the sensor.